#!/usr/bin/env python
# encoding: utf-8

from PyQt5.QtWidgets import *
from PyQt5.QtGui import *
from PyQt5.QtCore import *

from common.libs.ComDeal import WATER_CMD
from common.libs.MathFunc import MathFunc 
from common.libs.Log import logging

from ui.motorControl import Ui_Dialog

class MotorControlDialog(QDialog, Ui_Dialog):
    signal_movePos = pyqtSignal(float,float,float)
    signal_setLimit = pyqtSignal(float,float,float,float,float,float)
    signal_serial_send = pyqtSignal(list)
    signal_update_pos = pyqtSignal(float,float,float)
    signal_task = pyqtSignal(str)
    # 坐标变量
    err_x = 0
    err_y = 0
    err_z = 0
    real_x = 0
    real_y = 0
    real_z = 0
    pre_x = 0
    pre_y = 0
    pre_z = 0
    pulse_x = 0
    pulse_y = 0
    pulse_z = 0
    def __init__(self,parent=None,serial=''):
        super(MotorControlDialog, self).__init__(parent)
        self.setupUi(self)
        self.doubleSpinBox_x.setRange(-300,300)
        self.doubleSpinBox_y.setRange(-300,300)
        self.doubleSpinBox_z.setRange(-400,400)
        self.pushButton_movePos.clicked.connect(self.movePos)
        self.pushButton_setZero.clicked.connect(self.cmdAxisReset)
        self.pushButton_backZero.clicked.connect(self.backZero)
        self.pushButton_stop.clicked.connect(self.cmdMoveStop)
        self.pushButton_readLimit.clicked.connect(self.cmdReadLimit)
        self.pushButton_setLimit.clicked.connect(self.cmdSetLimit)
        self.serial = serial

        self.limit_x1 = 0
        self.limit_x2 = 0
        self.limit_y1 = 0
        self.limit_y2 = 0
        self.limit_z1 = 0
        self.limit_z2 = 0

    def backZero(self):
        self.cmdMoveAxisPos(0,0,0)

    def movePos(self):
        x = self.doubleSpinBox_x.value()
        y = self.doubleSpinBox_y.value()
        z = self.doubleSpinBox_z.value()
        self.cmdMoveAxisPos(x,y,z)

    def updatePos(self, x, y, z):
        self.signal_update_pos.emit(x,y,z)
        self.real_x = x
        self.real_y = y
        self.real_z = z
        self.label_x.setText('X:%3.3f'%float(x))
        self.label_y.setText('Y:%3.3f'%float(y))
        self.label_z.setText('Z:%3.3f'%float(z))
        self.updateLimit(
            self.limit_x1 - float(x),
            self.limit_x2 - float(x),
            self.limit_y1 - float(y),
            self.limit_y2 - float(y),
            self.limit_z1 - float(z),
            self.limit_z2 - float(z),
        )

    def updateLimit(self,x1,x2,y1,y2,z1,z2):
        self.lineEdit_limit_x1.setText(str(x1))
        self.lineEdit_limit_x2.setText(str(x2))
        self.lineEdit_limit_y1.setText(str(y1))
        self.lineEdit_limit_y2.setText(str(y2))
        self.lineEdit_limit_z1.setText(str(z1))
        self.lineEdit_limit_z2.setText(str(z2))

    def cmdReadLimit(self):
        """读取限位"""
        cmd = WATER_CMD['读取限位']
        self.signal_serial_send.emit(cmd)

    def cmdMoveStop(self):
        """移动停止"""
        cmd = WATER_CMD['停止']
        self.signal_serial_send.emit(cmd)

    def cmdMoveAxisPos(self, x,y,z):
        """移动到点位"""
        self.err_x = float(x) - self.real_x
        self.err_y = float(y) - self.real_y
        self.err_z = float(z) - self.real_z
        if self.err_x < 0:
            cmd_x = ('32')
        else:
            cmd_x = ('31')
        if self.err_y < 0:
            cmd_y = ('34')
        else:
            cmd_y = ('33')
        if self.err_z < 0:
            cmd_z = ('36')
        else:
            cmd_z = ('35')
        logging.info('电机正在移动到坐标: (%s,%s,%s)' % (x, y, z))
        cmd = WATER_CMD['电机走步']
        data_x = hex(abs(int('%.0f'%(self.err_x * self.pulse_x)))).replace('0x', '').zfill(8).upper()
        data_y = hex(abs(int('%.0f'%(self.err_y * self.pulse_y)))).replace('0x', '').zfill(8).upper()
        data_z = hex(abs(int('%.0f'%(self.err_z * self.pulse_z)))).replace('0x', '').zfill(8).upper()
        cmd[4] = cmd_x + data_x + cmd_y + data_y + cmd_z + data_z
        self.signal_serial_send.emit(cmd)
    
    def cmdRunTask(self, task):
        '''发送执行任务命令'''
        road = task['road']
        # 将负数转换成十六进制
        tohex = lambda val, nbits: hex((val + (1 << nbits)) % (1 << nbits))
        cmd_axis = hex(int(abs(len(road)))).replace('0x', '').zfill(4).upper()
        for a in road:
            data_x = tohex(int('%.0f'%(a[0] * self.pulse_x)), 16).replace('0x', '').zfill(4).upper()
            data_y = tohex(int('%.0f'%(a[1] * self.pulse_y)), 16).replace('0x', '').zfill(4).upper()
            data_z = tohex(int('%.0f'%(a[2] * self.pulse_z)), 16).replace('0x', '').zfill(4).upper()
            cmd_axis += data_x + data_y + data_z
        cmd = WATER_CMD['任务坐标']
        cmd[4] = cmd_axis
        logging.info('任务坐标:')
        logging.info(road)
        self.signal_serial_send.emit(cmd)

    def cmdStopTask(self):
        '''紧急停止'''
        cmd = WATER_CMD['移动停止']
        cmd[4] = ''
        self.signal_serial_send.emit(cmd)

    def cmdAxisReset(self):
        '''复位坐标系'''
        cmd = WATER_CMD['复位坐标']
        cmd[4] = ''
        self.signal_serial_send.emit(cmd)

    def cmdSetLimit(self, x1,x2,y1,y2,z1,z2):
        self.limit_x1 = float(self.lineEdit_limit_x1.text())
        self.limit_x2 = float(self.lineEdit_limit_x2.text())
        self.limit_y1 = float(self.lineEdit_limit_y1.text())
        self.limit_y2 = float(self.lineEdit_limit_y2.text())
        self.limit_z1 = float(self.lineEdit_limit_z1.text())
        self.limit_z2 = float(self.lineEdit_limit_z2.text())
        def tohex_8(t):
            return hex(0xFFFFFFFF&int(t*self.pulse_x)).replace('0x', '').zfill(8).upper()
        cmd = WATER_CMD['设置限位']
        cmd[4] ='31' + tohex_8(self.limit_x1) +\
                '32' + tohex_8(self.limit_x2) +\
                '33' + tohex_8(self.limit_y1) +\
                '34' + tohex_8(self.limit_y2) +\
                '35' + tohex_8(self.limit_z1) +\
                '36' + tohex_8(self.limit_z2)
        self.signal_serial_send.emit(cmd)

    def axisReset(self):
        self.real_x = 0
        self.real_y = 0
        self.real_z = 0
        self.pre_x = 0
        self.pre_y = 0
        self.pre_z = 0
        self.updatePos(0,0,0)
        QMessageBox.information(self, '提示','坐标复位成功！',QMessageBox.Yes)

    def getAxis(self, data):
        """数据中处理坐标信息"""
        x = MathFunc.hexStrToNeg(data[2:10])
        y = MathFunc.hexStrToNeg(data[12:20])
        z = MathFunc.hexStrToNeg(data[22:30])
        axis_x = float('%.3f' % (x / self.pulse_x))
        axis_y = float('%.3f' % (y / self.pulse_y))
        axis_z = float('%.3f' % (z / self.pulse_z))
        return axis_x,axis_y,axis_z

    def serial_deal(self, cmd, data):
        if cmd == '44':  # 复位坐标
            self.updatePos(0,0,0)
            QMessageBox.information(self, '提升','坐标复位成功！',QMessageBox.Yes)
        if cmd == '50':  # 任务移动测量
            if data == 'FF':  # 完成
                logging.info("连续测量任务完成")
            else:
                x,y,z = self.getAxis(data[2:])
                self.updatePos(x, y, z)
        if cmd == '54': # 任务紧急停止
            x,y,z = self.getAxis(data[2:])
            self.updatePos(x, y, z)
        if cmd == '55': # 停止
            x,y,z = self.getAxis(data[2:])
            self.updatePos(x, y, z)
        if cmd == '51':  # 移动
            if data == 'FF':  # 完成
                if self.real_x==0 and self.real_y == 0 and self.real_z == 0:
                    QMessageBox.information(self, '提升','已经回到原点！',QMessageBox.Yes)
                self.signal_task.emit("MOVE_POS")
            else:
                x,y,z = self.getAxis(data[2:])
                self.updatePos(x, y, z)

from .RayGraphWidget import RayCanvasWidget
from ui.moveShow import Ui_DockWidget
class MoveShowDock(QDockWidget, Ui_DockWidget):
    signal_movePos = pyqtSignal(float,float,float)
    def __init__(self,parent=None):
        super(MoveShowDock, self).__init__(parent)
        self.setupUi(self)
        self.rayGL = RayCanvasWidget()
        self.verticalLayout.addWidget(self.rayGL)

    def updatePos(self, x, y, z):
        self.label_x.setText('X:%3.3f'%float(x))
        self.label_y.setText('Y:%3.3f'%float(y))
        self.label_z.setText('Z:%3.3f'%float(z))
        self.rayGL.updatePos(x,y,z)



if __name__ == '__main__':
    import sys
    app = QApplication(sys.argv)
    # win = MotorControlDock()
    win = MotorControlWidget()
    win.show()
    app.exec_()

